/**
 * @author Victoria
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStationLCD;

public class RobotAngleOptimizer {

    private static final boolean DEBUGGING = true;
    
    //RAO motor
    private Victor motor;
    
    //Limit switches
    private DigitalInput lsTop, lsBottom;
    private DriverStationLCD.Line raoPrintLine, raoPrintLineTop;
    
    //Joystick and buttons
    Joystick rightDriveJoystick;
    private int extendButton, retractButton;
    
    //Constant for speed
    private static final double MOTOR_SPEED = -1.0;

    public RobotAngleOptimizer(Joystick joystick, int motorPort, int lsTop, int lsBottom, int pyramidTilt, int groundTilt, DriverStationLCD.Line printLine) {
        //Initializing RAO motor
        motor = new Victor(motorPort);
        
        //Initializing limit switches
        this.lsTop = new DigitalInput(lsTop);
        this.lsBottom = new DigitalInput(lsBottom);
        
        //Initializing joystick and buttons
        rightDriveJoystick = joystick;
        extendButton = pyramidTilt;
        retractButton = groundTilt;
        
        raoPrintLine = printLine;
    }

    //Controls RAO when buttons pressed
    public void tilt() {
        if (rightDriveJoystick.getRawButton(extendButton)) { //&& !lsBottom.get()) { //Tilting towards pyramid; RAO moves down if bottom limit switch not hit
            motor.set(MOTOR_SPEED);
        } else if (rightDriveJoystick.getRawButton(retractButton)) { //&& !lsTop.get()) { //Tilting down to ground; RAO moves back up if top limit switch not hit
            motor.set(-MOTOR_SPEED);
        } else { //No buttons pressed
            motor.set(0.0);
        }
        ;
        DriverStationLCD.getInstance().println(raoPrintLine, 1, "RAO at top: " + lsBottom.get() + "RAO at bottom: " + lsTop.get());
        DriverStationLCD.getInstance().updateLCD();
        
        //Prints limit switch values to output
        if (DEBUGGING) { System.out.println("RAO Top LS: " + lsTop.get() + " RAO Bottom LS: "+ lsBottom.get());}
    }
}